(*
 * XML preprocessing for radio-control parameters
 *
 * Copyright (C) 2003 Pascal Brisset, Antoine Drouin
 * Copyright (C) 2017 Gautier Hattenberger, Cyril Allignol
 *
 * This file is part of paparazzi.
 *
 * paparazzi is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2, or (at your option)
 * any later version.
 *
 * paparazzi is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with paparazzi; see the file COPYING.  If not, write to
 * the Free Software Foundation, 59 Temple Place - Suite 330,
 * Boston, MA 02111-1307, USA.
 *
 *)

open Printf
open Radio

let h_name = "RADIO_H"

(* Characters used in Gen_airframe.pprz_value *)
let check_function_name = fun s ->
  for i = 0 to String.length s - 1 do
    match s.[i] with
        'A'..'Z' | '0'..'9' | '_' -> ()
      | _ ->
        failwith (sprintf "Character '%c' not allowed in function name '%s'" s.[i] s)
  done


let norm1_ppm = fun c ->
  if c.neutral = c.min || c.neutral = c.max then
    sprintf "tmp_radio * (MAX_PPRZ / (float)(RC_PPM_SIGNED_TICKS_OF_USEC(%d-%d)))" c.max c.min, "0"
  else
    sprintf "tmp_radio * (tmp_radio >=0 ? (MAX_PPRZ/(float)(RC_PPM_SIGNED_TICKS_OF_USEC(%d-%d))) : (MIN_PPRZ/(float)(RC_PPM_SIGNED_TICKS_OF_USEC(%d-%d))))" c.max c.neutral c.min c.neutral, "MIN_PPRZ"

let gen_normalize_ppm_fir = fun out channels ->
  fprintf out "#define NormalizePpmFIR(_ppm, _rc) {\\\n";
  fprintf out "  static uint8_t avg_cpt = 0; /* Counter for averaging */\\\n";
  fprintf out "  int16_t tmp_radio;\\\n";
  List.iter
    (fun c ->
      let value, min_pprz = norm1_ppm c in
      if c.average then begin
        fprintf out "  _rc.avg_values[RADIO_%s] += _ppm[RADIO_%s];\\\n" c.cname c.cname
      end else begin
        fprintf out "  tmp_radio = _ppm[RADIO_%s] - RC_PPM_TICKS_OF_USEC(%d);\\\n" c.cname c.neutral;
        fprintf out "  _rc.values[RADIO_%s] = %s;\\\n" c.cname value;
        fprintf out "  Bound(_rc.values[RADIO_%s], %s, MAX_PPRZ); \\\n\\\n" c.cname min_pprz;
      end
    )
    channels;
  fprintf out "  avg_cpt++;\\\n";
  fprintf out "  if (avg_cpt == RC_AVG_PERIOD) {\\\n";
  fprintf out "    avg_cpt = 0;\\\n";
  List.iter
    (fun c ->
      if c.average then begin
        let value, min_pprz = norm1_ppm c in
        fprintf out "    tmp_radio = _rc.avg_values[RADIO_%s] / RC_AVG_PERIOD -  RC_PPM_TICKS_OF_USEC(%d);\\\n" c.cname c.neutral;
        fprintf out "    _rc.values[RADIO_%s] = %s;\\\n" c.cname value;
        fprintf out "    _rc.avg_values[RADIO_%s] = 0;\\\n" c.cname;
        fprintf out "    Bound(_rc.values[RADIO_%s], %s, MAX_PPRZ); \\\n\\\n" c.cname min_pprz;
      end
    )
    channels;
  fprintf out " }\\\n";
  fprintf out "}\n\n"

let norm1_ppm2 = fun c ->
  if c.neutral = c.min || c.neutral = c.max then
    sprintf "(tmp_radio * MAX_PPRZ) / (RC_PPM_SIGNED_TICKS_OF_USEC(%d-%d))" c.max c.min, "0"
  else
    sprintf "(tmp_radio >=0 ? (tmp_radio *  MAX_PPRZ) / (RC_PPM_SIGNED_TICKS_OF_USEC(%d-%d)) : (tmp_radio * MIN_PPRZ) / (RC_PPM_SIGNED_TICKS_OF_USEC(%d-%d)))" c.max c.neutral c.min c.neutral, "MIN_PPRZ"

let gen_normalize_ppm_iir = fun out channels ->
  fprintf out "#define NormalizePpmIIR(_ppm, _rc) {\\\n";
  fprintf out "  int32_t tmp_radio;\\\n";
  fprintf out "  int32_t tmp_value;\\\n\\\n";
  List.iter
    (fun c ->
      let value, min_pprz = norm1_ppm2 c in
      fprintf out "  tmp_radio = _ppm[RADIO_%s] - RC_PPM_TICKS_OF_USEC(%d);\\\n" c.cname c.neutral;
      fprintf out "  tmp_value = %s;\\\n" value;
      fprintf out "  Bound(tmp_value, %s, MAX_PPRZ); \\\n" min_pprz;
      if c.average then
        fprintf out "  _rc.values[RADIO_%s] = (pprz_t)((RADIO_FILTER * _rc.values[RADIO_%s] + tmp_value) / (RADIO_FILTER + 1));\\\n\\\n" c.cname c.cname
      else
        fprintf out "  _rc.values[RADIO_%s] = (pprz_t)(tmp_value);\\\n\\\n" c.cname
    )
    channels;
  fprintf out "}\n\n"


let generate = fun radio xml_file out_file ->
  let out = open_out out_file in
  fprintf out "/* This file has been generated by gen_radio from %s */\n" xml_file;
  fprintf out "/* Version %s */\n" (Env.get_paparazzi_version ());
  fprintf out "/* Please DO NOT EDIT */\n\n";
  fprintf out "#ifndef %s\n" h_name;
  fprintf out "#define %s\n\n" h_name;
  fprintf out "#define RADIO_NAME %s\n\n" radio.name;
  fprintf out "#define RADIO_CTL_NB %s\n\n" (string_of_int (List.length radio.channels));
  fprintf out "#define RADIO_FILTER 7\n\n";

  List.iteri (fun i c ->
    check_function_name c.cname;
    fprintf out "#define RADIO_%s %d\n" c.cname i;
    fprintf out "#define RADIO_%s_NEUTRAL %d\n" c.cname c.neutral;
    fprintf out "#define RADIO_%s_MIN %d\n" c.cname c.min;
    fprintf out "#define RADIO_%s_MAX %d\n\n" c.cname c.max;
    if (not c.reverse && (c.max < c.min)) || (c.reverse && (c.min < c.max))
      then failwith (sprintf "Error: \"%s\" radio channel: max must be superior to min! Use reverse=\"1\" to reverse the channel." c.cname)
  ) radio.channels;

  let ppm_pulse_type = match radio.pulse_type with
  | PositivePulse -> "POSITIVE"
  | NegativePulse -> "NEGATIVE"
  in
  fprintf out "#define PPM_PULSE_TYPE PPM_PULSE_TYPE_%s\n" ppm_pulse_type;
  fprintf out "#define PPM_DATA_MIN_LEN (%dul)\n" radio.data_min;
  fprintf out "#define PPM_DATA_MAX_LEN (%dul)\n" radio.data_max;
  fprintf out "#define PPM_SYNC_MIN_LEN (%dul)\n" radio.sync_min;
  fprintf out "#define PPM_SYNC_MAX_LEN (%dul)\n\n" radio.sync_max;

  gen_normalize_ppm_fir out radio.channels;
  gen_normalize_ppm_iir out radio.channels;

  fprintf out "\n#endif // %s\n" h_name;

  close_out out


